/**
 * @file motion_al.h
 * @brief Motion module application layer interfaces
*/

#pragma once
#include <vector>
#include <mutex>
#include <memory>
#include "zuc/nml_intf/zucpos2.h"
#include "axisgroup/ag_config.h"

#include "zuc/nml_intf/zuc_nml_rob.hh"
#define CART_DIM 6

namespace axisgroup {
class Robot;
};
namespace mot::al {
const size_t MAX_ROBOT_PROXY_CNT = 3;

class TOPP_member
{
public:
    BlendSegPath blend_seg_jpos;  //这里存放从转接点到目标点的离散关节位置  motionflag topp所用队列
    double last_tc_goal_jointpos[RobJointData<double>::MAX_AXIS] = {0};       // for toppra: 上一个tc段目标点的关节位置         motionflag
    double last_tc_blend_jointpos[RobJointData<double>::MAX_AXIS] = {0};      // for toppra: 上一个tc段转接点的关节位置         motionflag
    double last_tc_blend_jointpos_sec[RobJointData<double>::MAX_AXIS] = {0};  //for toppra: 上一个tc段转接点下一离散点的关节位置  motionflag
    double last_tc_refit_jpos[6][RobJointData<double>::MAX_AXIS] = {0};

    double last_tc_blend_r = 0.0;  // for toppra:上一个tc段的转接半径,如果实际运动没有转接成功，需要置零  motionflag
    double cur_tc_blend_r = 0.0;   // for toppra:当前转接段的转接半径  motionflag

    double last_final_pathp = 0.0;  //有转接的时候,转接段路径点的值在上一运动段转接点处的路径值开始进行叠加  motionflag toppuse
    int lasttc_jointNum = 0;        //上一个关节运动段运动最长的关节             motionflag toppuse
    int lasttc_left_len = 0;        //上一关节运动段从转接点到目标点离散点的个数   motionflag toppuse
    int first_command = 1;          //motionflag  topp相关
    int userset_use_toppra = 0;     //规划使用TOPP标志位
    int userset_filter_size = 0;    //motionflag topp相关
    int useradd_tor_cons = 1;       //TOPP 添加力矩限制？
    double usertor_portion = 1.0;   //topp
    int cur_blend_len;
    int cur_blend_left;
    int cur_last_blend_len;
    int blend_jpos_num;
    int jpos_len;
    double disjlen;
    double disllen;
    double disclen;
    double smooth_para;
    int robot_joint_num;
    TcPathInfo blend_path_info[1001];
    TcPathInfo path_info[1001];

    double tcp_vel_limit_toppra = 8000.0;
    int kinematic_error = 0;  //motionflag  运动学出错标志
    ConstraintsToppra cons_toppra;

    int zucTrajGetFilterSize(int filter_size);
    int zucTrajGetAddTorCons(int add_tor_cons);
    int zucTrajGetTorPortion(double tor_portion);
    int zucTrajSetDisJLen(double dislen);
    int zucTrajSetDisLLen(double dislen);
    int zucTrajSetDisCLen(double dislen);
    int zucTrajSetSmoothPara(double smooth_para);
    void get_lasttc_blend_jpos(ZucJPose* blend_jpos_for_fit);
    int movj_get_toppra_params();
    int movc_get_toppra_params();
    int get_unit_vector(double* vector, double mag, double* unit_vector);
    PmCartesian get_movc_center(PmCartesian A, PmCartesian B, PmCartesian C);
    int get_movc_planning_params(MovcStruct* circle, ZucPose const* const start, ZucPose const* const mid, ZucPose const* const end);
    int get_movc_pos(MovcStruct* circle, const double portion, ZucPose* discrete_cartpos);
    int quaternion_interp(PmRotationMatrix* const Rot_0, PmRotationMatrix* const Rot_1, const double t, ZucPose* discrete_cartpos);
    void rot2quat(PmRotationMatrix* const m, double* q);
    int get_line_position(ZucPose* start, double* dist_unit_vector, const double len, ZucPose* line_position);
    int get_line_gesture(ZucPose* start, ZucPose* end, const double len, ZucPose* gesture);
    int rpy_quat_convert(double r, double p, double y, double* quat);
    void quartic_basic(const double t, double* B);
    int get_quat_inv(double* quat, double* quat_inv);
    int quat_quat_mult(double* quat1, double* quat2, double* quat_out);
    void power_quat(const double* q, const double t, double* Q);
    void log_quat(const double* q, double* Q);
    void exp_quat(const double* q, double* Q);
    void norm_quat(const double* q, double* q_n);
    int quat_rpy_convert(double* quat, PmRpy* const rpy);
    int ClearBlendQueue();
    int BlendEnqueue(BlendJpos element_added);
    int GetBlendFront(BlendJpos* element_front);
    int sget_blend_pathvalue_coefs_csaps(const int discrete_num, const double start_path_data, const ZucJPose* jpos_for_fit);
    // int get_pathvalue_coefs_pycubic(const int discrete_num, const ZucJPose * jpos_for_fit);
    int get_toppra_related_params(const int discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit);
    int get_blend_path_information(const int blend_jpos_len, ConstraintsToppra& constraints);
    int blend_joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    int blend_joint_acc_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    int get_path_information(const int jpos_len, ConstraintsToppra& constraints);
    int joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    int joint_acc_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    double get_min(double a, double b);
    double get_max(double a, double b);
    void init_DYN();
};

class ZUC_JOINT_SAFETY
{
public:
    int jointNum;
    int paramType;  // 0: joint limit; 1: joint vel limit; 2: joint err limit
    JointPosLimit posLimt;
    float jointVelLimit;
    float jointErrLimit;
    int type;  // 0:set 1:get
};

class ZUC_ROBOT_SAFETY
{
public:
    // For internal NML/CMS use only.
    int custome;      // 0: customize type, 1: no limit, 2:loose , 3: medium ,4:strict
    int lastCustome;  // 0: customize type, 1: no limit, 2:loose , 3: medium ,4:strict
    int paramType;    // 0: all params; 1: momentum limit; 2: tcp velocity limit; 3: robot power limit
    float momentumLimit;
    float tcpVelLimit;
    float robotPowerLimit;
    float stoppingDistance;
    float stoppingTime;
    int type;  // 0:set 1:get
};

typedef struct zucmot_safety_cfg_t
{
    double negative_limit[RobJointData<double>::MAX_AXIS];
    double positive_limit[RobJointData<double>::MAX_AXIS];
    double jointVelLimit[RobJointData<double>::MAX_AXIS];
    double jointErrLimit[RobJointData<double>::MAX_AXIS];
    double init_traj_jpos[RobJointData<double>::MAX_AXIS];
    double init_jpos_tol;
    int motRunMode;        // 0-real device, 1: real device without CAN check, 2: simulation
    int clsn_sensitivity;  // collision sensitivity
    double momentumLimitValue;
    double tcpVelLimit;
    double dragTcpSpeedLimit;
    double robotPowerLimit;
    CollisionOption clsnOpt;
    SafetyFuncDIMap sFuncDISettings;
    SafetyFuncDOMap sFuncDOSettings;
    AutoWorkConfig autoWork;
    int onekeyenable;
    int autoprogram;
    SafeZone zone;
    // zuc_set_attitude_safety attitudesafety;
    reduce_param_config reduce_param;
    double stoppingDist;
    double stoppingTime;
    double elbow_center_offset;
    double elbow_sphere_radius;
    int force_limit;
    int joint_collision_sensitivity[RobJointData<double>::MAX_AXIS];
    int collision_enable;
    ClsnStopAcc clsn_stop_acc;
} SafetyConfig;

class ZUC_REDUCE_SAFETY
{
public:
    // For internal NML/CMS use only.
    double tcp_speed;
    double elbow_speed;
    double momentum_limit;
    double power_limit;
    double stopping_time;
    double stopping_dist;
};

typedef struct JointConfig_t
{
    int Inited;
    unsigned char Type;  // non-zero means joint called init
    double Units;
    double MaxVel;
    double MaxAccel;
    double MinLimit;
    double MaxLimit;
    double ErrLimit;
    double HMaxVel;  // hard maximum velocity, in ZUXXX.ini
} JointConfig_t;

typedef struct AxisConfig_t
{
    int Inited;
    unsigned char Type;
    double MaxVel;
    double MaxAccel;
    double Home;
    double MinLimit;
    double MaxLimit;
} AxisConfig_t;

typedef struct TrajConfig_t
{
    int Inited;  // non-zero means traj called init
    int Joints;
    double MaxAccel;
    double MaxVel;
    double MaxJointJerk;
    double MaxCarteJerk;
    int DeprecatedAxes;
    int AxisMask;
    double LinearUnits;
    double AngularUnits;
    int MotionId;
    //added by alex.li aat 2018.08.03
    double MaxJerk;
    double baseRx;
    double baseRy;
    double baseRz;
    ZucPose toolOffset[16];
    ZucPose userOffset[16];
    int safeZoneOpenEnable;
    int safeZoneAutoEnable;
    double SafezoneElbowRadius;
    double SafezoneElbowOffest;
    int filterType;
    int mmf_buf_size;
    double mmf_ka;
    double mmf_kv;
    double mmf_kp;
    double nlf_vp;
    double nlf_ap;
    double nlf_jp;
    double nlf_vr;
    double nlf_ar;
    double nlf_jr;
    double lpf_cutoff_freq;
} TrajConfig_t;

typedef struct zuc_jog_t
{                                                  /* for jogcont/incr/movj/movl/movc/abs/stop */
    ZucPose cartPose;                              /* target cartesian pose*/
    ZucPose cartPoseEx;                            /* extra cartesian pose, for movc mid pose*/
    double posCmd[RobJointData<double>::MAX_AXIS]; /* joint pos cmd*/
    double vel;                                    /* max velocity */
    double acc;                                    /* max acceleration */
    double offset;                                 /* for jog incr/abs */
    double tol;
    int executing_line_id;
    int end_cond[3];
    int moveOpt;
    int circle_cnt; /* movc circle count*/
    int is_cpos;
    double ori_vel;
    double ori_acc;
    int circle_mode;
} zuc_jog_t;

class ZUC_JOINT_STAT
{
public:
    int status;
    // For internal NML/CMS use only.

    int servo_version;
    int clsn_sensitivity;
    int first_poweron_date;

    // configuration parameters
    unsigned char jointType;  // ZUC_JOINT_LINEAR, ZUC_JOINT_ANGULAR
    double units;             // units per mm, deg for linear, angular
    double backlash;
    double minPositionLimit;
    double maxPositionLimit;
    double maxFerror;
    double minFerror;
    double maxVel;
    double maxAcc;

    // dynamic status
    double ferrorCurrent;   // current following error
    double ferrorHighMark;  // magnitude of max following error
    /*! \todo FIXME - is this really position, or the DAC output? */
    double output;               // commanded output position
    double input;                // current input position
    double velocity;             // current velocity
    double acceleration;         // current acceleration
    unsigned char inpos;         // non-zero means in position
    unsigned char homing;        // non-zero means homing
    unsigned char homed;         // non-zero means has been homed
    unsigned char fault;         // non-zero means axis amp fault
    unsigned char enabled;       // non-zero means enabled
    unsigned char minSoftLimit;  // non-zero means min soft limit exceeded
    unsigned char maxSoftLimit;  // non-zero means max soft limit exceeded
    unsigned char minHardLimit;  // non-zero means min hard limit exceeded
    unsigned char maxHardLimit;  // non-zero means max hard limit exceeded

    //伺服错误统计信息
    uint32_t servo_warn_date;  //统计日期
    unsigned int servo_warn_8384_cnt;
    unsigned int servo_warn_8384_daliy_cnt;
    unsigned int servo_warn_8385_cnt;
    unsigned int servo_warn_8385_daliy_cnt;
    unsigned int servo_warn_8387_cnt;
    unsigned int servo_warn_8387_daliy_cnt;
    unsigned int servo_warn_8388_cnt;
    unsigned int servo_warn_8388_daliy_cnt;
    unsigned int servo_warn_7001_cnt;
    unsigned int servo_warn_7001_daliy_cnt;
};

typedef struct
{
    ZUC_JOINT_SAFETY jointLimit[RobJointData<double>::MAX_AXIS * 2];
    ZUC_ROBOT_SAFETY robotSafe[6];  // 0:custom limit, 1:old_cuntom limit, 2:no limit, 3:loose limit, 4:medium limit,5: strict limit
    SafetyConfig safetyCfg;
    ZUC_REDUCE_SAFETY reduceSafe;
} Config_safety;
class RobotALIntf
{
public:
    struct Config
    {
        int robotModel;
        DHParam dhParam;
        JointConfig_t JointConfig[RobJointData<double>::MAX_AXIS];  //轴组信息是不是放到父类里？
        AxisConfig_t AxisConfig[MAX_GROUP];
        TrajConfig_t TrajConfig;
        Config_safety safety;
    };
    struct Status_Motion
    {
        double shrunkenPose[RobJointData<double>::MAX_AXIS];   //出厂姿态
        double stretchedPose[RobJointData<double>::MAX_AXIS];  //打开姿态
        double initialPose[RobJointData<double>::MAX_AXIS];    //安全姿态
        double initPoseTol;
        double homeVel;
        RobotModelData robot_model_data;
        int full_dh_com;
        int already_inpos;
        int motion_type;
        zuc_jog_t jog;
        int compliantType;
        //可能用不到
        int inpos;
    };
    struct Status
    {
        axisgroup::AgConfig::RobotConfig robot_config;  // 机器人配置

        uint32_t serial_num;  //机器人序列号
        int robot_joint_num;  // 机器人理论关节数量
        uint32_t rob_id;      //机器人索引值
        bool is_sim;          // 是否为仿真模式
        bool is_alive;        //是否在线
        bool is_power_on;     //上电状态是否完成
        bool is_powering;     //上电中
        bool is_enable;       //使能状态是否完成
        bool is_enabling;     //上使能中
        bool is_drag;         //是否拖动状态
        bool is_error;        //是否有错误
        bool is_cmd_done;     //指令插补完成
        bool is_act_inpos;    //反馈是否到位
        double rapid_scale;   // coord 模式的倍率（0-1）
        bool is_paused;       // motion 是否处于暂停状态

        RobJointVal cmd_jpos;        // motion 规划指令位置
        RobJointVal cmd_servo_jpos;  // 伺服指令位置，可能是由motion层的位置叠加而来
        RobJointVal act_jpos;        // 伺服反馈位置
        ZucPose cmd_tcp_pose;        // TCP指令位置, 叠加力控、传送带跟踪等
        ZucPose act_tcp_pose;        // TCP反馈位置
        ZucPose desired_tcp_pose;    // TCP期望位置

        RobJointData<double> cmd_tor;  // 指令力矩
        RobJointData<double> act_tor;  // 反馈力矩

        RobotDH robot_dh;  // 机器人DH参数，可能时机型文件中的，也可能是伺服读取的
        int plan_mode;     // 当前所处的规划模式

        uint8_t cur_tool_id;
        uint8_t cur_usrframe_id;
        uint8_t base_frame_id;

        struct LimitStat
        {
            RobJointData<bool> joint_vel;  //关节速度约束
            bool moment;                   //动量约束
            bool power;                    //功率约束
            bool tcpvel;                   //tcp速度约束
            bool reduce;                   //缩减状态
            uint8_t percent_lv;            // 0-未激活倍率模式 1-一级倍率模式 2-二级倍率模式
        } limit_stat;                      //运动处于的限制状态
        struct ErrorMsgs
        {
            uint32_t rob_ecode;
            RobJointData<uint32_t> joint_ecode;
        } error_msgs;

        ZucPose act_flange_pose;
        Config config;
        Status_Motion motion;
        JointTraj payloadIdentifyTraj[4];
        int userset_use_toppra;
        int unable_use_toppra;
        double trajCycleTime;
        // PyObject* pmodule;
        int joint;  //被操作的关节？？？
        int currSimMode;
        //可能没用的
        unsigned int safetyCRC;
        int operation_mode;         // can switch between auto and manual
        int operation_mode_option;  // operation mode option, 1 refers to enable operation mode
        int protective_stop;
    };

    TOPP_member TOPP;

public:
    virtual ~RobotALIntf() = default;
    virtual int init(uint8_t grpid) = 0;
    virtual int boot() = 0;
    virtual int shutdown() = 0;
    virtual void update() = 0;

public:  // 产生或改变运动的指令
    virtual int do_enable(bool en) = 0;
    virtual int do_stop() = 0;  //cat2 stop
    virtual int do_jog_joint_cont(uint8_t id, int nr, double vel) = 0;
    virtual int do_jog_joint_incr(uint8_t id, int nr, double incr, double vel) = 0;
    virtual int do_jog_joint_abs(uint8_t id, int nr, double pos, double vel) = 0;
    virtual int do_jog_tcp_cont(uint8_t id, int nr, double vel) = 0;
    virtual int do_jog_tcp_incr(uint8_t id, int nr, double incr, double vel) = 0;
    virtual int do_jog_tcp_abs(uint8_t id, int nr, double pos, double vel) = 0;
    virtual int do_jog_movj(RobJointVal jpos, MoveSpeed speed) = 0;
    virtual int do_drag(bool en, uint8_t mode) = 0;  //mode 拖动模式,0-关节拖动,1-传感器笛卡尔拖动
    virtual int do_movj(RobJointVal jpos, MoveSpeed speed, MovjParam movj_param) = 0;
    virtual int do_movl(RobTarget target, MoveSpeed speed, MovLParam movl_param, int tool_id, int usrframe_id) = 0;
    virtual int do_movc(RobTarget target, RobTarget mid, MoveSpeed speed, Movc_Param movc_param, int tool_id, int usrframe_id) = 0;
    virtual int do_movs(Movs_Param movs_param) = 0;
    virtual int do_pause() = 0;
    virtual int do_resume() = 0;
    virtual int do_servo_move(bool en) = 0;
    virtual int do_sync_conveyor(bool en) = 0;
    virtual int do_ft_xxx() = 0;  //力控运动
    virtual int zucJogStop(int nr, int jjogmode) = 0;
    virtual int zucTrajStep() = 0;

    virtual int
    zucTrajCircularMove(ZucPose end, PM_CARTESIAN center, PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc, double jerk) = 0;

public:  //配置具有临时性的参数
    virtual int set_scale(double scale) = 0;

public:  // 配置参数
    virtual int config_default_pos() = 0;
    virtual int config_servo_move() = 0;
    virtual int config_base_frame_id() = 0;
    virtual int config_jog_param() = 0;
    // virtual int config_kine_param(DHParam dhParam) = 0;
    virtual int config_dyna_param(DynParam dynParam) = 0;

public:
    // virtual int zucGetModelFromSerialNum(unsigned int serialNum, int* robotModel) = 0;
    // virtual int zucGetModelFromSerialNum() = 0;

    //负载辨识
    virtual int zucSetPayloadIdentifyTrajStartPos(int trajId, JointTraj jointTraj, int saveOpt) = 0;  //motionflag 设置负载辨识轨迹起点
    virtual int zucSetPayloadIdentifyTrajEndPos(int trajId, JointTraj jointTraj, int saveOpt) = 0;
    virtual int zucStartPayloadIdentifyTraj(int trajId, JointTraj jointTraj, int withPayload) = 0;
    //摩擦力辨识
    virtual int zucStopIdentifyFriction(int friction_identify_joint_index, int saveOpt) = 0;
    virtual int zucStartIdentifyFriction(int friction_identify_joint_index, int saveOpt) = 0;

    //设置task 运动相关参数
    virtual int zucSetDHCom(int full_dh_com) = 0;
    virtual int zucTrajSetPayload(Payload payload) = 0;
    virtual int zucTrajSetPayload(Payload payload, bool saveFile = true) = 0;
    virtual int zucSetRobot7dof_invflag(int inv_flag) = 0;
    virtual int zucSetRobotfull_dh_flag(int full_dh_flag) = 0;
    virtual int zucSetRobot7dof_arm_angle(double arm_angle) = 0;
    virtual int SetRobot7dofinv_inform(int inv_flag, double arm_angle) = 0;
    virtual int SetRobotFull_DH_flag(int full_dh_flag) = 0;
    virtual int SetRobot7dof_kine_param_alpha_k(double k, double alpha) = 0;

    //转接参数设置
    virtual int zucSetPBblendMode(int pb_blend_mode) = 0;
    virtual int zucSetMovejMovelBlend(int enable) = 0;
    virtual int zucSetMovejMovejBlend(int enable) = 0;
    virtual int zucTrajSetTermCond(int cond, double tolerance) = 0;
    virtual int zucSetupArcBlends(int arcBlendEnable,
                                  int arcBlendFallbackEnable,
                                  int arcBlendOptDepth,
                                  int arcBlendGapCycles,
                                  double arcBlendRampFreq,
                                  double arcBlendTangentKinkRatio) = 0;

    //配置轨迹信息
    virtual int zucTrajSetJoints(int joints) = 0;
    virtual int zucTrajSetAxes(int axismask) = 0;
    virtual int zucTrajSetMode(int mode) = 0;
    virtual int zucTrajInit() = 0;
    virtual int zucTrajEnable() = 0;
    virtual int zucTrajDisable() = 0;
    virtual int zucSetPlannerJerk(double joint_jerk, double carte_jerk) = 0;
    virtual int zucTrajSetPlannerJerk(double joint_jerk, double carte_jerk, int saveOption) = 0;
    virtual int zucSetOperationModeOption(unsigned int enable) = 0;

    virtual int zucTrajDelay(double delay) = 0;

    //配置倍率相关参数
    virtual int zucTrajSetScale(double scale) = 0;
    virtual int zucTrajSetRapidScale(double scale) = 0;
    virtual int zucTrajSetFOEnable(unsigned char mode) = 0;
    virtual int zucTrajSetSOEnable(unsigned char mode) = 0;
    virtual int zucTrajSetAFEnable(unsigned char enable) = 0;

    //安全参数设置
    virtual double get_vel_by_stopdisttime(double jerk, int motion_type, int planner_type) = 0;
    virtual int zucSetReduceSafety(ZUC_MOTION_STAT* stat, reduce_param_config reduce_param) = 0;
    virtual int zucSetDragTcpSpeedLimitSetting(double dragTcpSpeedLimit) = 0;
    virtual int zucSetRobotTcpVelLimit(float tcpVelLimit) = 0;
    virtual int zucSetRobotStoppingDist(float stoppingDist) = 0;
    virtual int zucSetRobotStoppingTime(float stoppingTime) = 0;
    virtual int zucSetRobotElbowVelLimitReduce(double elbowVelLimitReduce) = 0;
    virtual int zucSetDragTcpSpeedLimit(double dragTcpSpeedLimit) = 0;
    virtual int zucSetRobotTcpVelLimitReduce(double tcpVelLimitReduce) = 0;
    virtual int zucSetRobotMomentumLimitReduce(double momentumLimitReduce) = 0;
    virtual int zucSetRobotPowerLimitReduce(double robotPowerLimitReduce) = 0;
    virtual int zucSetRobotStoppingDistReduce(double stopping_dist_reduce) = 0;
    virtual int zucSetRobotMomentumLimit(float momentumLimit) = 0;
    virtual int zucSetRobotStoppingTimeReduce(double stopping_time_reduce) = 0;
    // virtual int zucSetRobotSafety(ZUC_MOTION_STAT * stat, ZUC_ROBOT_SAFETY robot_safety) = 0;

    //关节安全参数
    virtual int zucSetLimitLinearVelocity(int enable) = 0;
    virtual int zucSetLimitJointVelocity(int enable) = 0;

    virtual double zucAxisGetMaxVelocity(int axis) = 0;
    //笛卡尔JOG相关参数设置
    virtual int zucSetCartJogAcc(double* step_jog_pos_acc, double* step_jog_ori_acc) = 0;

    //传送带
    virtual int zucTrackingConfig(int link_di, int com_cycle_num, double synch_acc, int lctsynch) = 0;
    virtual int zucTrajConveyorLinearEnable(double x, double y, double z, double pulseEquivalent, int convyr_type, double max_distance) = 0;
    virtual PmCartesian get_conveyor_center(PmCartesian A, PmCartesian B, PmCartesian C) = 0;
    virtual int zucTrajConveyorCircularEnable(double p1x,
                                              double p1y,
                                              double p1z,
                                              double p2x,
                                              double p2y,
                                              double p2z,
                                              double p3x,
                                              double p3y,
                                              double p3z,
                                              double pulseEquivalent,
                                              int convyr_type,
                                              int rotate_tool,
                                              double max_distance) = 0;
    virtual int zucTrajConveyorDisable() = 0;

    //topp 相关
    virtual int zucTrajGetUseTopp(int use_topp) = 0;
    virtual int toppraPyEnvInit() = 0;
    virtual void update_related_params() = 0;
    virtual void update_related_params2() = 0;
    virtual void get_last_pos() = 0;
    virtual int
    movl_get_toppra_params(ZucPose start_cartpos, KINEMATICS_FORWARD_FLAGS* toppra_fflags, KINEMATICS_INVERSE_FLAGS* toppra_iflags, double tmag_abc) = 0;
    virtual int get_joint_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit) = 0;
    virtual int get_movl_discrete_info(ZucPose start_cartpos,
                                       int& discrete_num,
                                       ZucJPose* jpos_for_fit,
                                       ZucJPose* blend_jpos_for_fit,
                                       KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                                       KINEMATICS_INVERSE_FLAGS* toppra_iflags) = 0;
    virtual int get_movl_discrete_info_abc(ZucPose start_cartpos,
                                           int& discrete_num,
                                           ZucJPose* jpos_for_fit,
                                           ZucJPose* blend_jpos_for_fit,
                                           KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                                           KINEMATICS_INVERSE_FLAGS* toppra_iflags,
                                           double tmag_abc) = 0;
    virtual int get_movc_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit) = 0;
    // virtual int set_constraints_toppra(ConstraintsToppra & constraints) = 0;
    // virtual int blend_joint_tor_constraints_params(const int i_th, const double delta_s_value, PathCoefs & pathcoefs, ConstraintsToppra & constraints) = 0;
    // virtual int blend_interpolate_discretization(const int blend_jpos_len) = 0;
    // virtual int joint_tor_constraints_params(const int i_th, const double delta_s_value, PathCoefs & pathcoefs, ConstraintsToppra & constraints) = 0;
    // virtual int interpolate_discretization(const int jpos_len) = 0;
    virtual int zucMotionSetRunMode(int mode) = 0;
    virtual int zucTrajRigidTap(ZucPose pos, double vel, double ini_maxvel, double acc) = 0;
    virtual int zucSetTrajSmooth(int trqLimitActive, int firFltActive) = 0;
    virtual int zucTrajSetRateBlendPara(int rate_blend, int topp_queue_len, int max_fw_len) = 0;
    virtual int zucToppraFinishLeftTc() = 0;
    virtual int zucToppraFirstCommand() = 0;

    //运动信息更新
    virtual int zucMotionSetDebug(int debug) = 0;
    virtual int zucMotionExitProtect(int type) = 0;
    virtual int zucMotionPercentageMode(int level) = 0;
    virtual int zucMotionPercentageModeLevelValue(double percentage_mode_level1_val, double percentage_mode_level2_val) = 0;
    virtual int zucMotReduceMode(int reduceMode) = 0;

    //机器人信息更新
    virtual int zucAdapt2Robot(unsigned int robotSerialNum) = 0;

    //tcp 计算相关 应该在这个类里么？
    virtual int zucTrajCalcTcp(const unsigned char id,
                               const unsigned char saveType,
                               const unsigned char statusFlag,
                               const double* jntPos1,
                               const double* jntPos2,
                               const double* jntPos3,
                               const double* jntPos4) = 0;
    virtual int zucTrajCalcTcp_six(const unsigned char id,
                                   const unsigned char saveType,
                                   const unsigned char statusFlag,
                                   const double* jntPos1,
                                   const double* jntPos2,
                                   const double* jntPos3,
                                   const double* jntPos4,
                                   const double* jntPos5,
                                   const double* jntPos6) = 0;

    //user 用户坐标系计算相关
    virtual int zucTrajCalcUser(const unsigned char id,
                                const unsigned char saveType,
                                const unsigned char statusFlag,
                                const double* jntPos1,
                                const double* jntPos2,
                                const double* jntPos3) = 0;

    //从类中get参数 封装成类之后这种函数应该没用了
    virtual double zucJointGetMaxVelocity(int joint) = 0;
    virtual double zucJointGetMaxAcceleration(int joint) = 0;
    virtual double zucTrajGetMaxVel(void) = 0;
    virtual double zucAxisGetMaxAcceleration(int axis) = 0;
    virtual double zucTrajGetMaxAcceleration(void) = 0;
    virtual double zucTrajGetMaxJointJerk(void) = 0;
    virtual double zucTrajGetMaxCarteJerk(void) = 0;

public:
    virtual Status& get_status() = 0;
};

class MotionProxy
{
public:
    static MotionProxy& instance()
    {
        static MotionProxy ins;
        return ins;
    }
    ~MotionProxy();
    int motion_init();
    std::shared_ptr<RobotALIntf> robot(uint8_t id);
    int add_robot(std::shared_ptr<RobotALIntf> rob);
    int delete_robot(uint8_t id);
    inline size_t robot_cnt() const { return robs.size(); }

private:
    MotionProxy();

private:
    std::vector<std::shared_ptr<RobotALIntf>> robs;
};

}  // namespace mot::al
